#ifndef OOPSMP_UTILS_GEOMETRY_JOINTS_JOINTS3D_H_
#define OOPSMP_UTILS_GEOMETRY_JOINTS_JOINTS3D_H_

#include <cstdlib>
#include <cmath>
#include "OOPSMP/Utils/Geometry/Joints/Joints.h"
#include "OOPSMP/Utils/Math/Topology/SE3Topology.H"
#include "OOPSMP/Utils/Math/Algebra/matrix44.h"

namespace OOPSMP {
	
	/**
	 *@brief Macro for class, class pointer, and const class pointer
	 *       declarations 
	 */
	ForwardClassDeclaration(Joints3D);
	
	/**
	 *@brief joints 3d
	 */ 
	class Joints3D : public Joints {

	public:
		
		/**
		 *@brief Constructor
		 */ 
		Joints3D() { setJointLength(0.0); }

	protected:
		
		/**
		 *@brief Returns transformation between two robot links
		 *       Joint type is described by parameter values
		 *		 transformation can be broken down to the product T1*T2*T3 (defined bellow)
		 *@param gamma value of gamma angle (around x-axis) in joint
		 *@param beta value of beta angle (around y-axis) in joint
		 *@param alpha value of alpha angle (around z-axis) in joint
		 *@param x value of movement along x-axis in joint
		 *@param y value of movement along y-axis in joint
		 *@param z value of movement along z-axis in joint
		 *@param link1_dims dimensions of link we transform from
		 *@param link2_dims dimensions of link we transform to
		 *@param matrix empty array to contain transformation
		 */
		double * toMatrix(double gamma, double beta, double alpha, double x, double y, double z, double *link1_dims, double *link2_dims, double * matrix) const {
			double sin_gamma = sin(/*OOPSMPdeg_to_rad_angle*/(gamma));
			double cos_gamma = cos(/*OOPSMPdeg_to_rad_angle*/(gamma));
														   
			double sin_beta  = sin(/*OOPSMPdeg_to_rad_angle*/(beta));
			double cos_beta  = cos(/*OOPSMPdeg_to_rad_angle*/(beta));
														   
			double sin_alpha = sin(/*OOPSMPdeg_to_rad_angle*/(alpha));
			double cos_alpha = cos(/*OOPSMPdeg_to_rad_angle*/(alpha));

			double l1 = link1_dims[0];
			double w1 = link1_dims[1];
			double d1 = link1_dims[2];

			double l2 = link2_dims[0];
			double w2 = link2_dims[1];
			double d2 = link2_dims[2];

			matrix[0]  = cos_alpha * cos_beta;
			matrix[1]  = (cos_alpha * sin_beta * sin_gamma) - (sin_alpha * cos_gamma);
			matrix[2]  = (cos_alpha * sin_beta * cos_gamma) + (sin_alpha * sin_gamma);
			matrix[3]  = jointLength / 2 + l1 / 2 + x 
						 + cos_alpha * cos_beta * 
						 (jointLength / 2 + l2 / 2);
			matrix[4]  = sin_alpha * cos_beta;
			matrix[5]  = sin_alpha * sin_beta * sin_gamma + cos_alpha * cos_gamma;
			matrix[6]  = sin_alpha * sin_beta * cos_gamma - cos_alpha * sin_gamma;
			matrix[7]  = y + cos_beta * sin_alpha * (jointLength / 2 + l2 / 2);
			matrix[8]  = -sin_beta;
			matrix[9]  = cos_beta * sin_gamma;
			matrix[10] = cos_beta * cos_gamma;
			matrix[11] = z - sin_beta * (jointLength / 2 + l2 / 2);
			matrix[15] = 1;

			return matrix;
		}

		double * toMatrixT1(double *link1_dims, double *matrix) const {

			double l1 = link1_dims[0];
			double w1 = link1_dims[1];
			double d1 = link1_dims[2];

			matrix[0]  = matrix[5] = matrix[10] = matrix[15] = 1;
			matrix[3]  = l1/2 + jointLength/2;

			return matrix;
		}

		double * toMatrixT2(double gamma, double beta, double alpha, double x, double y, double z, double *matrix) const {

			double sin_gamma = sin(/*OOPSMPdeg_to_rad_angle*/(gamma));
			double cos_gamma = cos(/*OOPSMPdeg_to_rad_angle*/(gamma));
														   
			double sin_beta  = sin(/*OOPSMPdeg_to_rad_angle*/(beta));
			double cos_beta  = cos(/*OOPSMPdeg_to_rad_angle*/(beta));
														   
			double sin_alpha = sin(/*OOPSMPdeg_to_rad_angle*/(alpha));
			double cos_alpha = cos(/*OOPSMPdeg_to_rad_angle*/(alpha));

			matrix[0]  = cos_alpha * cos_beta;
			matrix[1]  = (cos_alpha * sin_beta * sin_gamma) - (sin_alpha * cos_gamma);
			matrix[2]  = (cos_alpha * sin_beta * cos_gamma) + (sin_alpha * sin_gamma);
			matrix[3]  = x;
			matrix[4]  = sin_alpha * cos_beta;
			matrix[5]  = sin_alpha * sin_beta * sin_gamma + cos_alpha * cos_gamma;
			matrix[6]  = sin_alpha * sin_beta * cos_gamma - cos_alpha * sin_gamma;
			matrix[7]  = y;
			matrix[8]  = -sin_beta;
			matrix[9]  = cos_beta * sin_gamma;
			matrix[10] = cos_beta * cos_gamma;
			matrix[11] = z;
			matrix[15] = 1;

			return matrix;
		}

		double * toMatrixT3(double *link2_dims, double *matrix) const {

			double l2 = link2_dims[0];
			double w2 = link2_dims[1];
			double d2 = link2_dims[2];

			matrix[0]  = matrix[5] = matrix[10] = matrix[15] = 1;
			matrix[3] = l2/2 + jointLength/2;

			return matrix;
		}

	};
	
	/**
     *@brief Declare factory and specify in source file the functions that should
     *       be registered for plug-and-play functionality
     */
	DeclareNoInstanceFactory(Joints3D, JointsFactory);

}

#endif
